#!/usr/bin/env python
PACKAGE = "motion_velocity_optimizer"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("max_velocity", double_t, 0, "max velocity limit [m/s]", 20.0, 0.1, 30.0)
gen.add("max_accel",    double_t, 0, "max acceleration limit [m/ss]", 1.0, 0.01, 5.0)
gen.add("min_decel",    double_t, 0, "min deceleration limit [m/ss]", -1.0, -5.0, -0.01)

# curve parameters
gen.add("max_lateral_accel",           double_t, 0, "max lateral acceleration limit [m/ss]", 0.5, 0.001, 5.0)
gen.add("min_curve_velocity",          double_t, 0, "min velocity at lateral acceleration limit [m/s] (high priority than max_lateral_accel)", 2.74, 0.01, 30.0)
gen.add("decel_distance_before_curve", double_t, 0, "slow speed distance before a curve for lateral acceleration limit", 3.5, 0.0, 30.0)
gen.add("decel_distance_after_curve",  double_t, 0, "slow speed distance after a curve for lateral acceleration limit", 2.0, 0.0, 30.0)

# engage & replan parameters
gen.add("replan_vel_deviation",         double_t, 0, "velocity deviation to replan initial velocity [m/s]",  5.53, 0.0, 30.0)
gen.add("engage_velocity",              double_t, 0, "engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) ", 0.25, 0.01, 10.0)
gen.add("engage_acceleration",          double_t, 0, "engage acceleration [m/ss] (use this acceleration when engagement)", 0.1, 0.0, 5.0)
gen.add("engage_exit_ratio",            double_t, 0, "exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.", 0.5, 0.0, 0.999)
gen.add("stop_dist_to_prohibit_engage", double_t, 0, "the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]", 0.5, 0.0, 5.0)

# stop velocity
gen.add("stopping_velocity", double_t, 0, "change target velocity to this value before v=0 point [m/s]", 2.778, 0.0, 100.0)
gen.add("stopping_distance", double_t, 0, "distance for the stopping_velocity [m]", 0.0, 0.0, 30.0)

gen.add("extract_ahead_dist",  double_t, 0, "forward trajectory distance used for planning [m]", 200.0, 1.0, 500.0)
gen.add("extract_behind_dist", double_t, 0, "backward trajectory distance used for planning [m]", 5.0, 0.0, 100.0)
gen.add("delta_yaw_threshold", double_t, 0, "Allowed delta yaw between ego pose and trajectory pose [radian]", 1.0472, 0.0, 6.283)

# resample parameters
gen.add("resample_time",                    double_t, 0, "resample total time [s]", 10.0, 0.1, 100.0)
gen.add("resample_dt",                      double_t, 0, "resample time interval [s]", 0.1, 0.001, 10.0)
gen.add("max_trajectory_length",            double_t, 0, "max trajectory length for resampling [m]", 200.0, 1.0, 500.0)
gen.add("min_trajectory_length",            double_t, 0, "min trajectory length for resampling [m]", 30.0, 1.0, 500.0)
gen.add("min_trajectory_interval_distance", double_t, 0, "sample points-interval length [m]", 0.1, 0.001, 10.0)

# optimization parameters
gen.add("pseudo_jerk_weight", double_t, 0, "weight for smoothness cost", 100.0, 0.01, 1000000.0)
gen.add("over_v_weight",      double_t, 0, "weight for over-speed-limit cost", 100000.0, 0.01, 1000000.0)
gen.add("over_a_weight",      double_t, 0, "weight for over-accel-limit cost", 1000.0, 0.01, 1000000.0)

# switching objective for jerk
# gen.add("algorithm_type", str_t, 0, "Linf : use Linf as objective for jerk, L2: use L2 as objective for jerk", "L2")


exit(gen.generate(PACKAGE, "motion_velocity_optimizer", "MotionVelocityOptimizer"))
